#include "Pid.h"
void PiControl(PiInfo_t * xPiInfo)
{
    int16_t Result = 0;
    int16_t Err = xPiInfo->Ref - xPiInfo->Act;
    int16_t Out = (Err * xPiInfo->Kp_Q15) + xPiInfo->ErrSum_Q15;
    Out = Out >> 15;//Q15转为实际值

    if( Out > xPiInfo->OutMax ){
        Result = xPiInfo->OutMax;
    }else if( Out < xPiInfo->OutMin ){
        Result = xPiInfo->OutMin;        
    }else{
        Result = Out;
    }
    Err -= (Out - Result)*xPiInfo->Kc_Q15;
    xPiInfo->ErrSum_Q15 += Err;
}
int16_t Q15Err;
//全参数定点数，用户将数据全部归一化后传入
void PiControl_AllQ15(PiInfoQ15_t * xPiInfo)
{
    int32_t Q30temp;
    int16_t Q15Result = 0;
    /*int16_t*/ Q15Err = xPiInfo->Ref_Q15 - xPiInfo->Act_Q15;//Q15
    Q30temp = (Q15Err * xPiInfo->Kp_Q15) + xPiInfo->ErrSum_Q30;
    int16_t Q15Out = Q30temp >> 15;//Q15

    if( Q15Out > xPiInfo->OutMax_Q15 ){
        Q15Result = xPiInfo->OutMax_Q15;
    }else if( Q15Out < xPiInfo->OutMin_Q15 ){
        Q15Result = xPiInfo->OutMin_Q15;        
    }else{
        Q15Result = Q15Out;
    }

    xPiInfo->Out_Q15 = Q15Result;

    Q30temp = Q15Err * xPiInfo->Ki_Q15;
    Q30temp -= (Q15Out - Q15Result)*xPiInfo->Kc_Q15;//抗积分饱和
    xPiInfo->ErrSum_Q30 += Q30temp;
}
